
#include <string.h>
#include <stdlib.h>

#include "adxl345.h"
#include "pwm.h"
#include "adc.h"
#include "i2c.h"
#include "itg3200.h"
#include "physics.h"
#include "uart.h"

#include "user_interface.h"
#include "commands.h"
#include "imu_filter.h"
#include "filter.h"


uint8_t motors_on = 0x00;
uint32_t motor_speed = 0x00;

ui_cmd_t ui_cmd_tbl[] =
{
{ "help", 0, 0, user_interface_cmd_help, "Print all commands" },
{ "set_speed", 2, 2, user_interface_cmd_set_speed, "Set Motor speed" },
{ "i2c_r", 2, 2, user_interface_cmd_i2c_read, "read from i2c device" },
{ "i2c_w", 3, 3, user_interface_cmd_i2c_write, "write to i2c device" },
{ "get", 0, 0, user_interface_cmd_get_test_value, "get test values" },
{ "reboot", 0, 0, user_interface_cmd_reboot, "reboots the controller" },
{ "bat", 0, 0, user_interface_cmd_get_adc, "get battery voltage" },
{ "on", 0, 0, user_interface_cmd_on, "switch motors on" },
{ "last function", 0, 0, NULL, "this is a dummy"}};

/**
 * Command line interface: Help
 *
 * @param  uint8_t argc argument count
 * @param  int8_t **argv arguments
 * @return none
 *
 * @brief  show all available commands
 *
 */
void user_interface_cmd_help(uint8_t argc, int8_t **argv)
{
    uint8_t i = 0;

    while (ui_cmd_tbl[i].func != NULL)
    {
        ui_printf("\n\t%s  -  %s", ui_cmd_tbl[i].command, ui_cmd_tbl[i].description );
        i++;
    }
}


/**
 * Command line interface: Set motor speed
 *
 * @param  uint8_t argc argument count
 * @param  int8_t **argv arguments
 * @return none
 *
 * @brief  sets motor speed depending on arguments
 *
 */
void user_interface_cmd_on(uint8_t argc, int8_t **argv)
{
	if (motors_on)
	{
		motors_on = 0;
	}
	else
	{
		motors_on = 1;
	}
}

/**
 * Command line interface: Set motor speed
 *
 * @param  uint8_t argc argument count
 * @param  int8_t **argv arguments
 * @return none
 *
 * @brief  sets motor speed depending on arguments
 *
 */
void user_interface_cmd_set_speed(uint8_t argc, int8_t **argv)
{
    uint8_t motor = strtoul(argv[0], NULL, 0);
    uint32_t speed = strtoul(argv[1], NULL, 0);

    /* does motor exist? */
    if (motor >= 6)
    {
        ui_printf("\nmotor %d not present\n", motor, speed);
    }
    /* motor in valid range */
    else
    {
    	motor_speed = speed;
    }
}

/**
 * Command line interface: Test function
 *
 * @param  uint8_t argc argument count
 * @param  int8_t **argv arguments
 * @return none
 *
 * @brief  -
 *
 */
void user_interface_cmd_get_test_value(uint8_t argc, int8_t **argv)
{

    ui_printf("\n%i %i %i ", (int32_t)toDegrees(imu_filter_get_roll()),
    		(int32_t)toDegrees(imu_filter_get_pitch()),
    		(int32_t)toDegrees(imu_filter_get_yaw()));

}

/**
 * Command line interface: Reboot
 *
 * @param  uint8_t argc argument count
 * @param  int8_t **argv arguments
 * @return none
 *
 * @brief  reboots the controller with a wdt reset
 *
 */
void user_interface_cmd_reboot(uint8_t argc, int8_t **argv)
{
	taskENTER_CRITICAL();
	while(1);

}

/**
 * Command line interface: Read I2C
 *
 * @param  uint8_t argc argument count
 * @param  int8_t **argv arguments
 * @return none
 *
 * @brief  reads data from i2c devices
 *
 */
void user_interface_cmd_i2c_read(uint8_t argc, int8_t **argv)
{
    uint8_t i2c_addr = strtoul(argv[0], NULL, 0);
    uint8_t reg = strtoul(argv[1], NULL, 0);
    uint8_t data[6];

    i2c_device_t dummy_device = {i2c_addr};

    i2c_read_burst(&dummy_device, reg, data);

    ui_printf("\n|0x%x|0x%x|\n ", reg, data[0]);


}

/**
 * Command line interface: Write I2C
 *
 * @param  uint8_t argc argument count
 * @param  int8_t **argv arguments
 * @return none
 *
 * @brief  Writes to i2c devices
 *
 */
void user_interface_cmd_i2c_write(uint8_t argc, int8_t **argv)
{
    uint8_t i2c_addr = strtoul(argv[0], NULL, 0);
    uint8_t reg = strtoul(argv[1], NULL, 0);
    uint8_t data = strtoul(argv[2], NULL, 0);


    i2c_device_t dummy_device = {i2c_addr};

    i2c_write(&dummy_device, reg, &data);

    ui_printf("\n|0x%x|0x%x|\n ", reg, data);
}


/**
 * Command line interface: Get battery voltage
 *
 * @param  uint8_t argc argument count
 * @param  int8_t **argv arguments
 * @return none
 *
 * @brief  gets the battery voltage
 *
 */
void user_interface_cmd_get_adc(uint8_t argc, int8_t **argv)
{
    ui_printf("\nBattery voltage: %i\n ", adc_get());
}

